#include "bt_interface.hpp"
#include <iostream>
#include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h"

namespace BT_Interface
{

	Ghost::Ghost()
	{
		name = "Ghost";
		m_ghostPairCallback = new btGhostPairCallback();
		m_ghostObject = new btPairCachingGhostObject();
		m_ghostObject->getWorldTransform().setIdentity();
		m_ghostObject->getWorldTransform().setOrigin(btVector3(0,0,0));
		m_ghostObject->setCollisionFlags(btCollisionObject::CF_NO_CONTACT_RESPONSE);//CF_CHARACTER_OBJECT);
		shape = NULL;
		convex_shape = NULL;
	}

	Ghost::~Ghost()
	{
		delete m_ghostObject;
		delete m_ghostPairCallback;
		if(shape)
			delete shape;
		if(convex_shape)
			delete convex_shape;
	}

	void Ghost::build_frustum(float aspect,float aleft,float aright,float atop, float abottom,float znear,float zfar)
	{

		const btScalar nearPlane = znear; 	// 1.0 is the value set by default
		const btScalar farPlane =  zfar; 	// 10000.0 is the value set by default
		const btScalar planesFraction = farPlane/nearPlane;
		const btScalar centralPlane = (farPlane-nearPlane) * 0.5f;	// Actually it's the center of the collision shape.
		btScalar left,right,top,bottom, farLeft,farRight,farBottom,farTop;

		left = aleft;
		right = aright;
		bottom = atop;
		top = abottom;


		farLeft = left * planesFraction;
		farRight = right * planesFraction;
		farBottom = bottom * planesFraction;
		farTop = top * planesFraction;

		convex_shape= new btConvexHullShape();

		btVector3 points[8]=
		{
			btVector3(left,top,centralPlane),
			btVector3(right,top,centralPlane),
			btVector3(left,bottom,centralPlane),
			btVector3(right,bottom,centralPlane),

			btVector3(farLeft,farTop,-centralPlane),
			btVector3(farRight,farTop,-centralPlane),
			btVector3(farLeft,farBottom,-centralPlane),
			btVector3(farRight,farBottom,-centralPlane)
		};

		for (unsigned int t=0; t<8; t++)
		{
			convex_shape->addPoint(points[t]);
		}

		shape = new btCompoundShape();
		const btVector3 v(0.,0.,-(nearPlane+centralPlane));
		const btQuaternion q = btQuaternion::getIdentity();// = btQuaternion::getIdentity();//(btVector3(0.,1.,0.),btRadians(180));
		btTransform T(q,v);
		shape->addChildShape(T/*.inverse()*/,convex_shape);	// Inverting now just results in swapping the vector sign.
		m_ghostObject->setCollisionShape(shape);
		m_ghostObject->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT | btCollisionObject::CF_NO_CONTACT_RESPONSE);//CF_CHARACTER_OBJECT);
	}

	Object::Object()
	{

		user_data = NULL;
		body = NULL;
		shape = NULL;
		motionState = NULL;

	}

	Object::Object(void *p)
	{
		user_data = p;
		body = NULL;
		shape = NULL;
		motionState = NULL;

	}

	Object::~Object()
	{
		release();
	}

	void Object::release()
	{
		if(body)
			delete body;
		if (shape)
			delete shape;
		if (motionState)
			delete motionState;
	}

	void Object::make_box(float mass,const btVector3& size,const btVector3& pos,const btQuaternion& orient)
	{
		release();
		btTransform startTransform;
		startTransform.setIdentity();
		startTransform.setOrigin(pos);
		startTransform.setRotation(orient);
		shape = new btBoxShape(size);
		btVector3 localInertia(0,0,0);
		shape->calculateLocalInertia(mass,localInertia);
		motionState = new btDefaultMotionState(startTransform);
		btRigidBody::btRigidBodyConstructionInfo cInfo(mass,motionState,shape,localInertia);
		body = new btRigidBody(cInfo);
		body->setLinearFactor(btVector3(1,1,1));
		body->setCcdMotionThreshold(1.);
		body->setCcdSweptSphereRadius(0.2f);
		body->setUserPointer(this);
	}

	void Object::make_compound(float mass,const btVector3& size,const btVector3& pos,const btQuaternion& orient)
	{
		release();
		btTransform tmp;
		tmp.setIdentity();
		btTransform startTransform;
		startTransform.setIdentity();
		startTransform.setOrigin(pos);
		startTransform.setRotation(orient);
		shape = new btCompoundShape();
		btCollisionShape *tshape = new btBoxShape(size);

        ((btCompoundShape*)shape)->addChildShape(tmp,tshape);
		btVector3 localInertia(0,0,0);

		shape->calculateLocalInertia(mass,localInertia);


		motionState = new btDefaultMotionState(startTransform);
		btRigidBody::btRigidBodyConstructionInfo cInfo(mass,motionState,shape,localInertia);
		body = new btRigidBody(cInfo);
		body->setLinearFactor(btVector3(1,1,1));
		body->setCcdMotionThreshold(1.);
		body->setCcdSweptSphereRadius(0.2f);
		body->setUserPointer(this);

	}



	void Object::make_plane(const btVector3& pos,const btVector3& norm,float smth)
	{
		release();
		float mass = 0;
		btTransform groundTransform;
		groundTransform.setIdentity();
		groundTransform.setOrigin(pos);
		shape = new btStaticPlaneShape(norm,smth);
		motionState = new btDefaultMotionState(groundTransform);
		btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,motionState,shape);
		body = new btRigidBody(rbInfo);
		body->setUserPointer(this);
		name = "Plane";
	}


	void Object::make_heightmap(Heightmap &hmap,const btVector3& pos)
	{
	    btScalar maxHeight = 0.0;

        float *data = new float[257*257];
        for(int i = 0; i < 257;i++)
            for(int j = 0; j < 257;j++)
            {
                data[i*257+j] = hmap.get_level(i,j);
                maxHeight = MAX(maxHeight,data[i*257+j]);
            }
        std::cout << maxHeight << std::endl;
        btHeightfieldTerrainShape* heightFieldShape = new btHeightfieldTerrainShape(257,257,data,maxHeight,1,true,true);


        heightFieldShape->setUseDiamondSubdivision(true);
        float scale = 2.0;
        btVector3 localScaling(1*scale,1,1*scale);
        heightFieldShape->setLocalScaling(localScaling);
        localScaling[1]=1.f;

		release();
		float mass = 0;
		btTransform groundTransform;
		groundTransform.setIdentity();
		groundTransform.setOrigin(btVector3(256 ,maxHeight/2, 256 ));

		//body->setLinearFactor(btVector3(1,1,1));

		motionState = new btDefaultMotionState(groundTransform);
		btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,motionState,heightFieldShape);
		body = new btRigidBody(rbInfo);
		body->setUserPointer(this);
		name = "Plane";
	}

    void Vehicle::add_vehicle_to_world(World &aworld)
    {
        world = &aworld;
        m_vehicleRayCaster = new btDefaultVehicleRaycaster(world->m_dynamicsWorld);
    }

	void Vehicle::set_body(float weight,const btVector3& size,const btVector3& pos,const btQuaternion& dir)
	{
        body.make_box(weight,size,pos,dir);

        world->add_object(body);
        m_vehicle = new btRaycastVehicle(m_tuning,body.body,m_vehicleRayCaster);

        body.body->setActivationState(DISABLE_DEACTIVATION);
        m_vehicle->setCoordinateSystem(0,1,2);
        world->m_dynamicsWorld->addVehicle(m_vehicle);

        body.body->setLinearVelocity(btVector3(0,0,0));
        body.body->setAngularVelocity(btVector3(0,0,0));
        m_vehicle->resetSuspension();
	}

	World::World()
	{
		m_collisionConfiguration = new btDefaultCollisionConfiguration();
		m_dispatcher = new	btCollisionDispatcher(m_collisionConfiguration);
		m_broadphase = new btSimpleBroadphase;
		m_solver = new btSequentialImpulseConstraintSolver;
		m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
		m_dynamicsWorld->setGravity(btVector3(0,-20,0));

		///reset some internal cached data in the broadphase
		m_dynamicsWorld->getBroadphase()->resetPool(m_dynamicsWorld->getDispatcher());
		m_dynamicsWorld->getConstraintSolver()->reset();
	}

	World::~World()
	{


		delete m_dynamicsWorld;
		delete m_collisionConfiguration;
		delete m_dispatcher;
		delete m_broadphase;
		delete m_solver;
	}

	struct filteredCollision:public btCollisionWorld::ClosestRayResultCallback
	{
		filteredCollision(const btVector3&	rayFromWorld,const btVector3&	rayToWorld)
		:btCollisionWorld::ClosestRayResultCallback(rayFromWorld,rayToWorld)
		{
		}

		virtual bool needsCollision(btBroadphaseProxy* proxy0) const
		{
			bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0;
			collides = collides && (m_collisionFilterGroup & proxy0->m_collisionFilterMask);
			collides = collides && ((btCollisionObject*)proxy0->m_clientObject)->getUserPointer();
			return collides;
		}
	};

	std::string World::query_ray(const btVector3& pos,const btVector3& ray)
	{


		filteredCollision rayCallback(pos,ray);
		m_dynamicsWorld->rayTest(pos,ray,rayCallback);
		if (rayCallback.hasHit())
		{
			btCollisionObject* body = (btCollisionObject*)(rayCallback.m_collisionObject);
			if(body->getUserPointer())
			{
				btVector3 vec(0,10.1,0.0);
				btVector3 imp = rayCallback.m_hitPointWorld + ((btRigidBody*) body)->getCenterOfMassPosition();
				imp *= 0.01;
				((btRigidBody*)body)->applyImpulse(imp,vec);
				return ((Object*)(body->getUserPointer()))->name;
			}
			else
				return "smthg";
		}
		return "None";
	}

	void World::query_ghost(Ghost &ghost,Query &qr)
	{

		qr.visible.resize(0);	// clear() is probably slower

		m_dynamicsWorld->getDispatcher()->dispatchAllCollisionPairs(ghost.m_ghostObject->getOverlappingPairCache(), m_dynamicsWorld->getDispatchInfo(), m_dynamicsWorld->getDispatcher());

		btBroadphasePairArray& collisionPairs = ghost.m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray();	//New
		const int	numObjects=collisionPairs.size();
		//std::cout << numObjects << std::endl;
		static btManifoldArray	m_manifoldArray;
		bool added;
		for(int i=0; i<numObjects; i++)
		{
			const btBroadphasePair& collisionPair = collisionPairs[i];
			m_manifoldArray.resize(0);
			collisionPair.m_algorithm->getAllContactManifolds(m_manifoldArray);

			added = false;
			for (int j=0; j<m_manifoldArray.size(); j++)
			{
				btPersistentManifold* manifold = m_manifoldArray[j];

				// Here we are in the narrowphase, but can happen that manifold->getNumContacts()==0:
				if (true)
				{
					for (int p=0; p<manifold->getNumContacts(); p++)
					{
						const btManifoldPoint&pt = manifold->getContactPoint(p);
						if (pt.getDistance() < 0.0)
						{
							// How can I be sure that the colObjs are all distinct ? I use the "added" flag.
							btCollisionObject* object =(btCollisionObject*) (manifold->getBody0() == ghost.m_ghostObject ? manifold->getBody1() : manifold->getBody0());
							if(object->getUserPointer())
								qr.visible.push_back((Object*)(object->getUserPointer()));

							added = true;
							break;
						}
					}
					if (added) break;
				}
				else if (manifold->getNumContacts()>0)
				{
					btCollisionObject* object =(btCollisionObject*) (manifold->getBody0() == ghost.m_ghostObject ? manifold->getBody1() : manifold->getBody0());
					if(object->getUserPointer())
						qr.visible.push_back((Object*)(object->getUserPointer()));

					break;
				}
			}
		}
	}

	void World::add_ghost(Ghost &ghost)
	{
		m_dynamicsWorld->addCollisionObject(ghost.m_ghostObject);
		m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->setInternalGhostPairCallback(ghost.m_ghostPairCallback);
	}

	void World::do_step(float sz)
	{
		m_dynamicsWorld->stepSimulation(sz);
	}

	void World::add_object(Object &object)
	{
		m_dynamicsWorld->addRigidBody(object.body);
	}
};
